from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration
import math

class Navigator:
    def __init__(self):
        if not rclpy.ok():
            rclpy.init()
        self.navigator = BasicNavigator()
        self.navigator.waitUntilNav2Active()

    def _navigate_to(self, x, y):
        """内部导航方法"""
        goal_pose = PoseStamped()
        goal_pose.header.frame_id = 'map'
        goal_pose.header.stamp = self.navigator.get_clock().now().to_msg()
        goal_pose.pose.position.x = x
        goal_pose.pose.position.y = y
        goal_pose.pose.orientation.w = -1.0
        
        self.navigator.goToPose(goal_pose)
        while not self.navigator.isTaskComplete():
            feedback = self.navigator.getFeedback()
            distance = self._calculate_distance(feedback.current_pose, goal_pose)
            self.navigator.get_logger().info(f'距离目标点还有: {distance*8:.2f} 米')
            if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
                self.navigator.cancelTask()
                return False
                
        return self.navigator.getResult() == TaskResult.SUCCEEDED

    @staticmethod
    def _calculate_distance(current_pose, goal_pose):
        dx = current_pose.pose.position.x - goal_pose.pose.position.x
        dy = current_pose.pose.position.y - goal_pose.pose.position.y
        return math.sqrt(dx*dx + dy*dy)

    def go_to_toilet(self):
        """去厕所"""
        return self._navigate_to(0.85, -0.3)

    def go_to_charging(self):
        """去充电桩"""
        return self._navigate_to(-0.15, 0.3)

# 测试代码
if __name__ == '__main__':
    nav = Navigator()
    nav.go_to_toilet()
    nav.go_to_charging()